/********************************************************************************************/
/* Fullhan Technology (Shanghai) Co., Ltd.                                                  */
/* Fullhan Proprietary and Confidential                                                     */
/* Copyright (c) 2017 Fullhan Technology Group Limited and its affiliated companies         */
/********************************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include "drv_cesa_ioctl.h"


static int g_fd = -1;
int32_t FH_UNF_CESA_Open(void)
{
	if (g_fd >= 0)
		return CESA_ALREADY_OPENED;

    g_fd = open("/dev/cesa", O_RDWR);
    
    if(g_fd >= 0)
        return FH_SUCCESS;
    
    return FH_FAILURE;
}

int32_t FH_UNF_CESA_Close(void)
{
    if (close(g_fd) == 0) {
		g_fd = -1;
		return FH_SUCCESS;
    } else
    	return FH_FAILURE;
}

int32_t FH_UNF_CESA_ADES_Create(uint32_t *p_session)
{
    CMD_ADES_CREATE_S cmd_create;

    ioctl(g_fd, CESA_IOC_ADES_CREATE, &cmd_create);

    if (FH_SUCCESS == cmd_create.ret)
    {
        *p_session = cmd_create.session;
    }

    return cmd_create.ret;
}

int32_t FH_UNF_CESA_ADES_Destroy(uint32_t session)
{
    CMD_ADES_DESTROY_S cmd_destroy;

    cmd_destroy.session = session;
    
    ioctl(g_fd, CESA_IOC_ADES_DESTROY, &cmd_destroy);

    return cmd_destroy.ret;
}

int32_t FH_UNF_CESA_ADES_Config(uint32_t session, FH_CESA_ADES_CTRL_S *p_ctrl)
{
    CMD_ADES_CTRL_S cmd_ctrl;
    
    cmd_ctrl.session = session;
    memcpy(&(cmd_ctrl.ctrl), p_ctrl, sizeof(FH_CESA_ADES_CTRL_S));
    
    ioctl(g_fd, CESA_IOC_ADES_CONFIG, &cmd_ctrl);

    return cmd_ctrl.ret;
}

int32_t FH_UNF_CESA_ADES_Process(uint32_t session, uint32_t p_src_addr, uint32_t p_dst_addr, uint32_t length)
{
	CMD_ADES_PROCESS_S cmd_process;

	cmd_process.session = session;
	cmd_process.p_src_addr = p_src_addr;
	cmd_process.p_dst_addr = p_dst_addr;
	cmd_process.length = length;

	ioctl(g_fd, CESA_IOC_ADES_PROCESS, &cmd_process);

	return cmd_process.ret;
}

int32_t FH_UNF_CESA_ADES_Encrypt(uint32_t session, uint32_t p_src_addr, uint32_t p_dst_addr, uint32_t length)
{
	CMD_ADES_PROCESS_S cmd_process;

	cmd_process.session = session;
	cmd_process.p_src_addr = p_src_addr;
	cmd_process.p_dst_addr = p_dst_addr;
	cmd_process.length = length;

	ioctl(g_fd, CESA_IOC_ADES_ENCRYPT, &cmd_process);

	return cmd_process.ret;
}

int32_t FH_UNF_CESA_ADES_Decrypt(uint32_t session, uint32_t p_src_addr, uint32_t p_dst_addr, uint32_t length)
{
	CMD_ADES_PROCESS_S cmd_process;

	cmd_process.session = session;
	cmd_process.p_src_addr = p_src_addr;
	cmd_process.p_dst_addr = p_dst_addr;
	cmd_process.length = length;

	ioctl(g_fd, CESA_IOC_ADES_DECRYPT, &cmd_process);

	return cmd_process.ret;
}

int32_t FH_UNF_CESA_ADES_Get_Info(uint32_t session, FH_CESA_ADES_CTRL_S *ctrl)
{
    CMD_ADES_INFO_S cmd_info;
    
    cmd_info.session = session;
    ioctl(g_fd, CESA_IOC_ADES_GET_INFO, &cmd_info);

	if (FH_SUCCESS == cmd_info.ret)
	{
		memcpy(ctrl, &(cmd_info.ctrl), sizeof(FH_CESA_ADES_CTRL_S));
	}

    return cmd_info.ret;
}

int32_t FH_UNF_CESA_ADES_Efuse_Key(uint32_t session, uint32_t *u32MapBuf, uint32_t u32MapBufSize)
{
	CMD_ADES_EFUSE_KEY_S cmd_efuse_key;
	int i;

	cmd_efuse_key.session = session;

	cmd_efuse_key.efuse_para.mode = CRYPTO_EX_MEM_SWITCH_KEY | CRYPTO_EX_MEM_4_ENTRY_1_KEY;
	cmd_efuse_key.efuse_para.map_size = u32MapBufSize;
	for(i = 0; i < u32MapBufSize; i++) {
		cmd_efuse_key.efuse_para.map[i].crypto_key_no = i;
		cmd_efuse_key.efuse_para.map[i].ex_mem_entry = u32MapBuf[i];
	}

	ioctl(g_fd, CESA_IOC_ADES_EFUSE_KEY, &cmd_efuse_key);

	return cmd_efuse_key.ret;
}

int32_t FH_UNF_CESA_ADES_Addr_Attr(uint32_t session,
					FH_CESA_ADES_ADDR_ATTR_E src_addr_attr,
					FH_CESA_ADES_ADDR_ATTR_E dst_addr_attr)
{
	CMD_ADES_ADDR_ATTR_S cmd_addr_attr;

	cmd_addr_attr.session = session;
	cmd_addr_attr.src_addr_attr = src_addr_attr;
	cmd_addr_attr.dst_addr_attr = dst_addr_attr;

	ioctl(g_fd, CESA_IOC_ADES_ADDR_ATTR, &cmd_addr_attr);

	return cmd_addr_attr.ret;
}




int32_t FH_UNF_CESA_SHA_Create(uint32_t *p_session)
{
    CMD_SHA_CREATE_S cmd_create;

    ioctl(g_fd, CESA_IOC_SHA_CREATE, &cmd_create);

    if (FH_SUCCESS == cmd_create.ret)
    {
        *p_session = cmd_create.session;
    }

    return cmd_create.ret;
}

int32_t FH_UNF_CESA_SHA_Destroy(uint32_t session)
{
    CMD_SHA_DESTROY_S cmd_destroy;

    cmd_destroy.session = session;
    
    ioctl(g_fd, CESA_IOC_SHA_DESTROY, &cmd_destroy);

    return cmd_destroy.ret;
}

int32_t FH_UNF_CESA_SHA_Init(uint32_t session, FH_CESA_SHA_CTRL_S *p_ctrl)
{
    CMD_SHA_INIT_S cmd_init;

    cmd_init.session = session;
    memcpy(&(cmd_init.ctrl), p_ctrl, sizeof(FH_CESA_SHA_CTRL_S));
    
    ioctl(g_fd, CESA_IOC_SHA_INIT, &cmd_init);

    return cmd_init.ret;
}

void *fh_mmz_new(uint32_t u32Size , uint32_t u32Align, int8_t *ps8MMZName, int8_t *ps8MMBName)
{
	return 0;
}
int32_t fh_mmz_delete(uint32_t u32PhysAddr)
{
	return 0;
}
void *fh_mmz_map(uint32_t u32PhysAddr, uint32_t u32Cached)
{
	return 0;
}
int32_t fh_mmz_unmap(uint32_t u32PhysAddr)
{
	return 0;
}
int32_t fh_mmz_get_phyaddr(void * pVir, uint32_t *pu32Phyaddr, uint32_t *pu32Size)
{
	return 0;
}

int32_t FH_UNF_CESA_SHA_Update(uint32_t session, uint8_t *p_msg, uint32_t length)
{
    CMD_SHA_UPDATE_S cmd_update;
    uint32_t phy_msg_addr = 0;
    uint8_t *vir_msg_addr = NULL;
    uint32_t size = 0;

    if (0 != fh_mmz_get_phyaddr(p_msg, &phy_msg_addr, &size))
    {
        phy_msg_addr = (uint32_t)fh_mmz_new(length, 16, NULL, NULL);
        if (0 == phy_msg_addr)
        {
            printf("fh_mmz_new for sha phy_msg_addr failed!\n");
            return FH_FAILURE;
        }

        vir_msg_addr = fh_mmz_map(phy_msg_addr, 0);
        if( NULL == vir_msg_addr )
        {
            fh_mmz_delete(phy_msg_addr);
            printf("fh_mmz_map for sha vir_msg_addr failed!\n");
            return FH_FAILURE;
        }

        memcpy(vir_msg_addr, p_msg, length);
    }
    else if ( size < length)
    {
        printf("src mmz size and length are not matched!\n");
        return FH_FAILURE;
    }


    cmd_update.session = session;
    cmd_update.p_msg = (uint8_t *)phy_msg_addr;
    cmd_update.length = length;

    
    ioctl(g_fd, CESA_IOC_SHA_UPDATE, &cmd_update);

    if (vir_msg_addr)
    {
        fh_mmz_unmap(phy_msg_addr);
        fh_mmz_delete(phy_msg_addr);
    }
    
    return cmd_update.ret;
}

int32_t FH_UNF_CESA_SHA_Final(uint32_t session, uint8_t *p_dgst)
{
    CMD_SHA_FINAL_S cmd_final;

    cmd_final.session = session;
    memset(cmd_final.p_dgst, 0, 32);
    
    ioctl(g_fd, CESA_IOC_SHA_FINAL, &cmd_final);

    if (0 == cmd_final.ret)
    {
        uint32_t i;

        for(i=20; i<32; i++)
        {
            if (0 != cmd_final.p_dgst[i])
            {
                break;
            }
        }

        if (i >= 32)
        {
            memcpy(p_dgst, cmd_final.p_dgst, 20);
        }
        else
        {
            memcpy(p_dgst, cmd_final.p_dgst, 32);
        }
    }

    return cmd_final.ret;
}

int32_t FH_UNF_CESA_SHA_Update_Start(uint32_t session, uint8_t *p_msg, uint32_t length)
{
    CMD_SHA_UPDATE_START_S cmd_update_start;

    cmd_update_start.session = session;
    cmd_update_start.p_msg = p_msg;
    cmd_update_start.length = length;
    
    ioctl(g_fd, CESA_IOC_SHA_UPDATE_START, &cmd_update_start);

    return cmd_update_start.ret;
}

int32_t FH_UNF_CESA_SHA_Update_Polling(uint32_t session, uint32_t timeout)
{
    CMD_SHA_UPDATE_POLLING_S cmd_update_polling;

    cmd_update_polling.session = session;
    cmd_update_polling.timeout = timeout;
    
    ioctl(g_fd, CESA_IOC_SHA_UPDATE_POLLING, &cmd_update_polling);

    return cmd_update_polling.ret;
}

int32_t FH_UNF_CESA_SHA_Update_Stop(uint32_t session)
{
    CMD_SHA_UPDATE_STOP_S cmd_update_stop;

    cmd_update_stop.session = session;
    
    ioctl(g_fd, CESA_IOC_SHA_UPDATE_STOP, &cmd_update_stop);

    return cmd_update_stop.ret;
}

int32_t FH_UNF_CESA_HMACSHA_Init(uint32_t session, FH_CESA_SHA_CTRL_S *p_ctrl)
{
    CMD_SHA_INIT_S cmd_init;
    CMD_SHA_UPDATE_S cmd_update;
    CMD_SHA_FINAL_S cmd_final;

    uint32_t phy_addr = 0;
    uint8_t *vir_addr = NULL;
    uint32_t size = 0;
    uint8_t BLOCK[64]={0};

    uint8_t *hmackey = p_ctrl->sha_para.hmac_key;
    uint32_t keysize = p_ctrl->sha_para.key_size;

    int32_t ret = CESA_SUCCESS;
    uint32_t i;

    if (0 != fh_mmz_get_phyaddr(hmackey, &phy_addr, &size))
    {
        phy_addr = (uint32_t)fh_mmz_new(keysize+64, 16, NULL, NULL);
        if (0 == phy_addr)
        {
            return -4001;
        }
        vir_addr = fh_mmz_map(phy_addr, 0);
        if( NULL == vir_addr )
        {
            fh_mmz_delete(phy_addr);
            return -4002;
        }
    }
    else if ( size < keysize)
    {
        return -4003;
    }

    
    

    if (keysize > 64)
    {
        cmd_init.session = session;
        memcpy(&(cmd_init.ctrl), p_ctrl, sizeof(FH_CESA_SHA_CTRL_S));
        ioctl(g_fd, CESA_IOC_SHA_INIT, &cmd_init);
        if (cmd_init.ret)
        {
            ret = cmd_init.ret;
            goto hmac_init_release;
        }



        cmd_update.session = session;
        memcpy(vir_addr, hmackey, keysize);
        cmd_update.p_msg = (uint8_t *)phy_addr;
        cmd_update.length = keysize;

        ioctl(g_fd, CESA_IOC_SHA_UPDATE, &cmd_update);
        if (cmd_update.ret)
        {
            ret = cmd_update.ret;
            goto hmac_init_release;
        }


        cmd_final.session = session;
        memset(cmd_final.p_dgst, 0, 32);
        ioctl(g_fd, CESA_IOC_SHA_FINAL, &cmd_final);
        if (cmd_final.ret)
        {
            ret = cmd_final.ret;
            goto hmac_init_release;
        }

        for (i=20; i<32; i++)
        {
            if (0 != cmd_final.p_dgst[i])
            {
                break;
            }
        }

        if (i >= 32)
        {
            memcpy(BLOCK, cmd_final.p_dgst, 20);
        }
        else
        {
            memcpy(BLOCK, cmd_final.p_dgst, 32);
        }
    }
    else
    {
        memcpy(BLOCK, hmackey, keysize);
    }

    for (i=0; i<64; i++)
    {
        p_ctrl->sha_para.o_key_pad[i] = 0x5C ^ BLOCK[i];
        p_ctrl->sha_para.i_key_pad[i] = 0x36 ^ BLOCK[i];
    }
    
    cmd_init.session = session;
    memcpy(&(cmd_init.ctrl), p_ctrl, sizeof(FH_CESA_SHA_CTRL_S));
    ioctl(g_fd, CESA_IOC_SHA_INIT, &cmd_init);
    if (cmd_init.ret)
    {
        ret = cmd_init.ret;
        goto hmac_init_release;
    }
    
    cmd_update.session = session;
    memcpy(vir_addr, p_ctrl->sha_para.i_key_pad, 64);
    cmd_update.p_msg = (uint8_t *)phy_addr;
    cmd_update.length = 64;
    ioctl(g_fd, CESA_IOC_SHA_UPDATE, &cmd_update);
    if (cmd_update.ret)
    {
        ret = cmd_update.ret;
        goto hmac_init_release;
    }
    
hmac_init_release:
    if (vir_addr)
    {
        fh_mmz_unmap(phy_addr);
        fh_mmz_delete(phy_addr);
    }
    return ret;
}

int32_t FH_UNF_CESA_HMACSHA_Update(uint32_t session, FH_CESA_SHA_CTRL_S *p_ctrl, uint8_t *p_msg, uint32_t length)
{
    CMD_SHA_UPDATE_S cmd_update;
    int32_t ret = CESA_SUCCESS;
    
    uint32_t phy_msg_addr = 0;
    uint8_t *vir_msg_addr = NULL;
    uint32_t size = 0;

    if (NULL == p_ctrl)
    {
        return -4004;
    }
    
    if (0 != fh_mmz_get_phyaddr(p_msg, &phy_msg_addr, &size))
    {
        phy_msg_addr = (uint32_t)fh_mmz_new(length, 16, NULL, NULL);
        if (0 == phy_msg_addr)
        {
            return -4005;
        }
        
        vir_msg_addr = fh_mmz_map(phy_msg_addr, 0);
        if( NULL == vir_msg_addr )
        {
           fh_mmz_delete(phy_msg_addr);
            return -4006;
        }

        memcpy(vir_msg_addr, p_msg, length);
    }
    else if (size < length)
    {
        return -4007;
    }

    cmd_update.session = session;
    memcpy(vir_msg_addr, p_msg, length);
    cmd_update.p_msg = (uint8_t *)phy_msg_addr;
    cmd_update.length = length;
    ioctl(g_fd, CESA_IOC_SHA_UPDATE, &cmd_update);
    if (cmd_update.ret)
    {
        printf(">>>>>>>>>>ret=%d\n",cmd_update.ret);
        ret = cmd_update.ret;
        goto hmac_update_release;
    }

hmac_update_release:
    if (vir_msg_addr)
    {
        fh_mmz_unmap(phy_msg_addr);
        fh_mmz_delete(phy_msg_addr);
    }
    return ret;
}

int32_t FH_UNF_CESA_HMACSHA_Final(uint32_t session, FH_CESA_SHA_CTRL_S *p_ctrl, uint8_t *p_dgst)
{
    CMD_SHA_INIT_S cmd_init;
    CMD_SHA_UPDATE_S cmd_update;
    CMD_SHA_FINAL_S cmd_final;
    int32_t ret = CESA_SUCCESS;
    uint32_t i;

    uint32_t phy_addr;
    uint8_t *vir_addr;

    phy_addr = (uint32_t)fh_mmz_new(64+32, 16, NULL, NULL);
    if (0 == phy_addr)
    {
        return -4008;
    }
    vir_addr = fh_mmz_map(phy_addr, 0);
    if( NULL == vir_addr )
    {
        fh_mmz_delete(phy_addr);
        return -4009;
    }

    cmd_final.session = session;
    memset(cmd_final.p_dgst, 0, 32);
    ioctl(g_fd, CESA_IOC_SHA_FINAL, &cmd_final);
    if (cmd_final.ret)
    {
        ret = cmd_final.ret;
        goto hmac_final_release;
    }

    cmd_init.session = session;
    memcpy(&(cmd_init.ctrl), p_ctrl, sizeof(FH_CESA_SHA_CTRL_S));
    ioctl(g_fd, CESA_IOC_SHA_INIT, &cmd_init);
    if (cmd_init.ret)
    {
        ret = cmd_init.ret;
        goto hmac_final_release;
    }

    cmd_update.session = session;
    memcpy(vir_addr, p_ctrl->sha_para.o_key_pad, 64);
    memcpy(vir_addr+64, cmd_final.p_dgst, 32);
    cmd_update.p_msg = (uint8_t *)phy_addr;
    cmd_update.length = 96;
    ioctl(g_fd, CESA_IOC_SHA_UPDATE, &cmd_update);
    if (cmd_update.ret)
    {
        ret = cmd_update.ret;
        goto hmac_final_release;
    }

    cmd_final.session = session;
    memset(cmd_final.p_dgst, 0, 32);
    ioctl(g_fd, CESA_IOC_SHA_FINAL, &cmd_final);
    if (cmd_final.ret)
    {
        ret = cmd_final.ret;
        goto hmac_final_release;
    }

    for (i=20; i<32; i++)
    {
        if (0 != cmd_final.p_dgst[i])
        {
            break;
        }
    }
    if (i >= 32)
    {
        memcpy(p_dgst, cmd_final.p_dgst, 20);
    }
    else
    {
        memcpy(p_dgst, cmd_final.p_dgst, 32);
    }

hmac_final_release:
    fh_mmz_unmap(phy_addr);
    fh_mmz_delete(phy_addr);
    return ret;
}

int32_t FH_UNF_CESA_RSA_Create(uint32_t *p_session)
{
    CMD_RSA_CREATE_S cmd_create;

    ioctl(g_fd, CESA_IOC_RSA_CREATE, &cmd_create);

    if (FH_SUCCESS == cmd_create.ret)
    {
        *p_session = cmd_create.session;
    }

    return cmd_create.ret;
}

int32_t FH_UNF_CESA_RSA_Destroy(uint32_t session)
{
    CMD_RSA_DESTROY_S cmd_destroy;

    cmd_destroy.session = session;
    
    ioctl(g_fd, CESA_IOC_RSA_DESTROY, &cmd_destroy);

    return cmd_destroy.ret;
}

int32_t FH_UNF_CESA_RSA_Process(uint32_t session, FH_CESA_RSA_CTRL_S *p_ctrl, uint8_t *p_src_addr, uint8_t *p_dst_addr, uint32_t src_length)
{
    CMD_RSA_PROCESS_S cmd_process;

    memset(&cmd_process, 0, sizeof(CMD_RSA_PROCESS_S));
    cmd_process.session = session;
    cmd_process.mod_length = p_ctrl->rsa_para.mod_length;
    cmd_process.exp_length = p_ctrl->rsa_para.exp_length;
    cmd_process.src_length = src_length;
    memcpy(cmd_process.m, p_ctrl->rsa_para.p_m, cmd_process.mod_length);
    memcpy(cmd_process.e, p_ctrl->rsa_para.p_e, cmd_process.exp_length);
    memcpy(cmd_process.src_addr, p_src_addr, src_length);
    
    ioctl(g_fd, CESA_IOC_RSA_PROCESS, &cmd_process);

    if(FH_SUCCESS == cmd_process.ret)
    {
        memcpy(p_dst_addr, cmd_process.dst_addr, cmd_process.mod_length);
    }

    return cmd_process.ret;
}








static int rng_fd = -1;
int32_t FH_UNF_CESA_Gen_Random_Bytes(uint32_t NumOfBytes, uint8_t*  pRandomBytes)
{
	if (rng_fd < 0)
	{
	#if 0
		/*
		 * TRNG: CONFIG_HW_RANDOM_FH @kernel
		 */
		rng_fd = open("/dev/hw_random", O_RDONLY);
	#else
		/*
		 * Pseudo-random Number Generator
		 */
		rng_fd = open("/dev/urandom", O_RDONLY);
	#endif
	}

	if(rng_fd < 0)
	{
		return -1;
	}

	if (NumOfBytes == (uint32_t)read(rng_fd, pRandomBytes, NumOfBytes))
	{
		return 0;
	}
	else
	{
		return -2;
	}
}

static int32_t MGF1(uint8_t *mask, int32_t len, uint8_t *seed, int32_t seedlen, uint32_t hashtype)
{
    uint32_t session;

    int32_t i, outlen = 0;
    uint8_t cnt[4];
    uint8_t md[64];// 20 32  only support sha1 sha256 
    int32_t mdlen;
    uint8_t update_buffer[256];

    FH_CESA_SHA_CTRL_S ctrl;



    FH_UNF_CESA_Open();
    FH_UNF_CESA_SHA_Create(&session);
    memset(&ctrl, 0, sizeof(FH_CESA_SHA_CTRL_S));
    ctrl.sha_para.algo_mode = (0==hashtype) ? FH_CESA_HASH_ALGO_MODE_SHA1 : FH_CESA_HASH_ALGO_MODE_SHA256;
    
    mdlen = (0==hashtype) ? 20 : 32;
    
    for (i = 0; outlen < len; i++)
    {
        cnt[0] = (unsigned char)((i >> 24) & 255);
        cnt[1] = (unsigned char)((i >> 16) & 255);
        cnt[2] = (unsigned char)((i >> 8) & 255);
        cnt[3] = (unsigned char)(i & 255);

        FH_UNF_CESA_SHA_Init(session, &ctrl);
        //FH_UNF_CESA_SHA_Update(cesa_handle, session, seed, (fh_u32)seedlen);
        //FH_UNF_CESA_SHA_Update(cesa_handle, session, cnt, 4);
        memcpy(update_buffer, seed, (uint32_t)seedlen);
        memcpy(update_buffer + seedlen, cnt, 4);
        FH_UNF_CESA_SHA_Update(session, update_buffer, (uint32_t)(seedlen+4));
        if (outlen + mdlen <= len)
        {
            FH_UNF_CESA_SHA_Final(session, mask + outlen);
            outlen += mdlen;
        }
        else
        {
            FH_UNF_CESA_SHA_Final(session, md);
            memcpy(mask + outlen, md, (uint32_t)(len - outlen));
            outlen = len;
        }
    }
    
    FH_UNF_CESA_SHA_Destroy(session);
    FH_UNF_CESA_Close();
    
    return 0;
}


static uint8_t sha1_null[20] =     {0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b, 0x0d, 0x32, 0x55, 0xbf, 0xef, 0x95, 0x60, 0x18, 0x90, 
                                             0xaf, 0xd8, 0x07, 0x09, 
                                           };
static uint8_t sha256_null[32] = {0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14, 0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24,
                                            0x27, 0xae, 0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c, 0xa4, 0x95, 0x99, 0x1b, 0x78, 0x52, 0xb8, 0x55,
                                           };
                                           
static inline unsigned int constant_time_msb(unsigned int a)
{
    return 0 - (a >> (sizeof(a) * 8 - 1));
}

static inline unsigned int constant_time_is_zero(unsigned int a)
{
    return constant_time_msb(~a & (a - 1));
}

static inline unsigned int constant_time_eq(unsigned int a, unsigned int b)
{
    return constant_time_is_zero(a ^ b);
}

static inline unsigned int constant_time_select(unsigned int mask,
                                                unsigned int a,
                                                unsigned int b)
{
    return (mask & a) | (~mask & b);
}

static inline int constant_time_select_int(unsigned int mask, int a, int b)
{
    return (int)(constant_time_select(mask, (unsigned)(a), (unsigned)(b)));
}

int32_t FH_UNF_CESA_RSA_padding_add_OAEP
(uint8_t *to, int32_t tlen,
uint8_t *from, int32_t flen,
uint32_t hashtype
)   
{
    int32_t i, emlen = tlen - 1;
    uint8_t *db, *seed;
    uint8_t seedmask[64],dbmask[256];
    int32_t mdlen = (0==hashtype) ? 20 : 32;

    if (flen > emlen - 2 * mdlen - 1)
    {
        printf("RSA_R_DATA_TOO_LARGE_FOR_KEY_SIZE\n");
        return -1;
    }

    if (emlen < 2 * mdlen + 1)
    {
        printf("RSA_R_KEY_SIZE_TOO_SMALL\n");
        return -1;
    }

    to[0] = 0;
    seed = to + 1;
    db = to + mdlen + 1;

    if(0 == hashtype)
    {
        memcpy(db, sha1_null, (uint32_t)mdlen);
    }
    else
    {
        memcpy(db, sha256_null, (uint32_t)mdlen);
    }

    memset(db + mdlen, 0, (uint32_t)(emlen - flen - 2 * mdlen - 1));
    db[emlen - flen - mdlen - 1] = 0x01;
    memcpy(db + emlen - flen - mdlen, from, (unsigned int) flen);
    
    if (FH_UNF_CESA_Gen_Random_Bytes((uint32_t)mdlen, seed) < 0)
    {
        printf("FH_UNF_CIPHER_RSA_padding_add_OAEP gen random seed failed\n");
        return -1;
    }
        
//#ifdef PKCS_TESTVECT
//memcpy(seed, "\xaa\xfd\x12\xf6\x59\xca\xe6\x34\x89\xb4\x79\xe5\x07\x6d\xde\xc2\xf0\x6c\xb5\x8f", 20);
//#endif

    MGF1(dbmask, emlen - mdlen, seed, mdlen, hashtype);
    for (i = 0; i < emlen - mdlen; i++)
        db[i] ^= dbmask[i];

    MGF1(seedmask, mdlen, db, emlen - mdlen, hashtype);
    for (i = 0; i < mdlen; i++)
        seed[i] ^= seedmask[i];

    return 0;
}

int32_t FH_UNF_CESA_RSA_padding_check_OAEP
(uint8_t *to, int32_t *tlen, 
uint8_t *from, int32_t flen, 
int32_t num,
uint32_t hashtype)
{
    int32_t i, dblen = 0, mlen = -1, one_index = 0, msg_index;
    uint32_t good, found_one_byte;
    uint8_t *maskedseed, *maskeddb;
    uint8_t db[256], em[256], seed[64], phash[64];
    int32_t mdlen = (0==hashtype)? 20 : 32;

    if (flen <= 0)
        return -1;

    /*
     * |num| is the length of the modulus; |flen| is the length of the
     * encoded message. Therefore, for any |from| that was obtained by
     * decrypting a ciphertext, we must have |flen| <= |num|. Similarly,
     * num < 2 * mdlen + 2 must hold for the modulus irrespective of
     * the ciphertext, see PKCS #1 v2.2, section 7.1.2.
     * This does not leak any side-channel information.
     */
    if (num < flen || num < 2 * mdlen + 2)
        return -1;

    dblen = num - mdlen - 1;
    /*
     * Always do this zero-padding copy (even when num == flen) to avoid
     * leaking that information. The copy still leaks some side-channel
     * information, but it's impossible to have a fixed  memory access
     * pattern since we can't read out of the bounds of |from|.
     *
     * TODO(emilia): Consider porting BN_bn2bin_padded from BoringSSL.
     */
    memset(em, 0, (uint32_t)num);
    memcpy(em + num - flen, from, (uint32_t)flen);
    /*
     * The first byte must be zero, however we must not leak if this is
     * true. See James H. Manger, "A Chosen Ciphertext  Attack on RSA
     * Optimal Asymmetric Encryption Padding (OAEP) [...]", CRYPTO 2001).
     */
    good = constant_time_is_zero(em[0]);
    
    maskedseed = em + 1;
    maskeddb = em + 1 + mdlen;

    
    MGF1(seed, mdlen, maskeddb, dblen, hashtype);
    for (i = 0; i < mdlen; i++)
    {
        seed[i] ^= maskedseed[i];
    }

    MGF1(db, dblen, seed, mdlen, hashtype);
    for (i = 0; i < dblen; i++)
    {
        db[i] ^= maskeddb[i];
    }

    if (0 == hashtype)
    {
        memcpy(phash, sha1_null, (uint32_t)mdlen);
    }
    else
    {
        memcpy(phash, sha256_null, (uint32_t)mdlen);
    }

    good &= constant_time_is_zero((uint32_t)memcmp(db, phash, (uint32_t)mdlen));
    for(i = 0; i < mdlen; i++)
    {
        good |=  db[i] ^ phash[i];
    }

    found_one_byte = 0;
    for (i = mdlen; i < dblen; i++) {
        /*
         * Padding consists of a number of 0-bytes, followed by a 1.
         */
        unsigned int equals1 = constant_time_eq(db[i], 1);
        unsigned int equals0 = constant_time_is_zero(db[i]);
        one_index = constant_time_select_int(~found_one_byte & equals1,
                                             i, one_index);
        found_one_byte |= equals1;
        good &= (found_one_byte | equals0);
    }

    good &= found_one_byte;

    /*
     * At this point |good| is zero unless the plaintext was valid,
     * so plaintext-awareness ensures timing side-channels are no longer a
     * concern.
     */
    if (!good)
        return -1;

	msg_index = one_index + 1;
	mlen = dblen - msg_index;

	memcpy(to, db + msg_index, (uint32_t)mlen);
	*tlen = mlen;

    return 0;
}

